#include <iostream>
#include "../inc/HoughSpace.h"

using namespace std;

HoughSpace::HoughSpace()
{
        bot.setXYZ(0,0,0);
        top.setXYZ(0,0,0);
        
        nbFrameA = 0;
        nbFrameB = 0;
        nbFrameR = 0;
        
        aFrameMax = 0;
        bFrameMax = 0;
        rFrameMax = 0;

        frameSizeA = 0;
        frameSizeB = 0;
        frameSizeR = 0;
        
        scoreMax = 0;
        scoreTotal = 0;
        
        frames = NULL;
}

HoughSpace::HoughSpace(int _mode, Point _bot, Point _top, float _a, float _b, float _r)
{
        bot = _bot;
        top = _top;
        
        aFrameMax = 0;
        bFrameMax = 0;
        rFrameMax = 0;
        
        scoreMax = 0;
        scoreTotal = 0;
        
        if ( _mode == RESOLUTION )
        {
                nbFrameA = (int)_a;
                nbFrameB = (int)_b;
                nbFrameR = (int)_r;
                
                frameSizeA = ( _top.getX() - _bot.getX() ) / nbFrameA;
                frameSizeB = ( _top.getY() - _bot.getY() ) / nbFrameB;
                frameSizeR = ( _top.getZ() - _bot.getZ() ) / nbFrameR;
        }
        
        if ( _mode == SIZE )
        {
                frameSizeA = _a;
                frameSizeB = _b;
                frameSizeR = _r;
                
                nbFrameA = (int) (( _top.getX() - _bot.getX() ) / frameSizeA) + 1;
                nbFrameB = (int) (( _top.getY() - _bot.getY() ) / frameSizeB) + 1;
                nbFrameR = (int) (( _top.getZ() - _bot.getZ() ) / frameSizeR) + 1;
        }
        
        if ( _mode != SIZE && _mode != RESOLUTION )
        {
                cout<<"Mauvais parametre pour la création de HoughSpace : seuls RESOLUTION et SIZE sont acceptés comme mode"<<endl;
        }
        
        frames = (HoughFrame***) malloc ( nbFrameA * sizeof ( HoughFrame** ) );
        for ( int i = 0 ; i < nbFrameA ; i++ )
        {
                frames[i] = (HoughFrame**) malloc ( nbFrameB * sizeof ( HoughFrame* ) );
                
                for ( int j = 0 ; j < nbFrameB ; j++ )
                {
                        frames[i][j] = (HoughFrame*) malloc ( nbFrameR * sizeof (HoughFrame) );
                        
                        for ( int k = 0 ; k < nbFrameR ; k++ )
                        {
                                Point pBot, pTop;
                                
                                pBot.setXYZ( bot.getX() + ( i * frameSizeA), bot.getY() + ( j * frameSizeB ), bot.getZ() + ( k * frameSizeR ));
                                pTop.setXYZ( pBot.getX() + frameSizeA, pBot.getY() + frameSizeB, pBot.getZ() + frameSizeR );
                                
                                frames[i][j][k].setPointBot( pBot );
                                frames[i][j][k].setPointTop( pTop );
                                
                                frames[i][j][k].setScore( 0 );
                        }
                }
        }
}

HoughSpace::~HoughSpace()
{
        for ( int i = 0 ; i < nbFrameA ; i++ )
        {
                for ( int j = 0 ; j < nbFrameB ; j++ )
                {
                        free(frames[i][j]);
                }
                
                free (frames[i]);
        }
        
        free(frames);
}

void HoughSpace::init(int _mode, Point _bot, Point _top, float _a, float _b, float _r)
{
        if ( frames != NULL )
        {
                cout<<"Erreur, l'espace de Hough a déjà été initialisé"<<endl;
                return;
        }

        bot = _bot;
        top = _top;
        
        aFrameMax = 0;
        bFrameMax = 0;
        rFrameMax = 0;
        
        scoreMax = 0;
        scoreTotal = 0;
        
        if ( _mode == RESOLUTION )
        {
                nbFrameA = (int)_a;
                nbFrameB = (int)_b;
                nbFrameR = (int)_r;
                
                frameSizeA = ( _top.getX() - _bot.getX() ) / nbFrameA;
                frameSizeB = ( _top.getY() - _bot.getY() ) / nbFrameB;
                frameSizeR = ( _top.getZ() - _bot.getZ() ) / nbFrameR;
        }
        
        if ( _mode == SIZE )
        {
                frameSizeA = _a;
                frameSizeB = _b;
                frameSizeR = _r;
                
                nbFrameA = (int) (( _top.getX() - _bot.getX() ) / frameSizeA) + 1;
                nbFrameB = (int) (( _top.getY() - _bot.getY() ) / frameSizeB) + 1;
                nbFrameR = (int) (( _top.getZ() - _bot.getZ() ) / frameSizeR) + 1;
        }
        
        if ( _mode != SIZE && _mode != RESOLUTION )
        {
                cout<<"Mauvais parametre pour la création de HoughSpace : seuls RESOLUTION et SIZE sont acceptés comme mode"<<endl;
        }
        
        frames = (HoughFrame***) malloc ( nbFrameA * sizeof ( HoughFrame** ) );
        for ( int i = 0 ; i < nbFrameA ; i++ )
        {
                frames[i] = (HoughFrame**) malloc ( nbFrameB * sizeof ( HoughFrame* ) );
                
                for ( int j = 0 ; j < nbFrameB ; j++ )
                {
                        frames[i][j] = (HoughFrame*) malloc ( nbFrameR * sizeof (HoughFrame) );
                        
                        for ( int k = 0 ; k < nbFrameR ; k++ )
                        {
                                Point pBot, pTop;
                                
                                pBot.setXYZ( bot.getX() + ( i * frameSizeA), bot.getY() + ( j * frameSizeB ), bot.getZ() + ( k * frameSizeR ));
                                pTop.setXYZ( pBot.getX() + frameSizeA, pBot.getY() + frameSizeB, pBot.getZ() + frameSizeR );
                                
                                frames[i][j][k].setPointBot( pBot );
                                frames[i][j][k].setPointTop( pTop );
                                
                                frames[i][j][k].setScore( 0 );
                        }
                }
        }
}

void HoughSpace::fill(vector <Circle> _tabCircles)
{
        int a, b, r;
        scoreMax = 0;

        if(_tabCircles.size() > 0)
        {
                for(int i=0; i < (int)_tabCircles.size(); i++)
                {
                        _tabCircles[i].setABR(_tabCircles[i].getA() - bot.getX(), _tabCircles[i].getB() - bot.getY(), _tabCircles[i].getR() - bot.getZ());

                        a = (int)((_tabCircles[i].getA())/frameSizeA);
                        b = (int)((_tabCircles[i].getB())/frameSizeB);
                        r = (int)((_tabCircles[i].getR())/frameSizeR);

                        if ( a >= nbFrameA )
                                a = nbFrameA-1;
                        
                        if ( b >= nbFrameB )
                                b = nbFrameB-1;
                        
                        if ( r >= nbFrameR )
                                r = nbFrameR-1;
                        
                        _tabCircles[i].setABR(_tabCircles[i].getA() + bot.getX(), _tabCircles[i].getB() + bot.getY(), _tabCircles[i].getR() + bot.getZ());

                        frames[a][b][r].incrementScore();

                        if(frames[a][b][r].getScore() > scoreMax)
                        {
                                scoreMax = frames[a][b][r].getScore();
                                aFrameMax = frames[a][b][r].getBotPoint().getX() + frameSizeA/2;
                                bFrameMax = frames[a][b][r].getBotPoint().getY() + frameSizeB/2;
                                rFrameMax = frames[a][b][r].getBotPoint().getZ() + frameSizeR/2;
                        }
                        
                }
        }
}

void HoughSpace::threshold(vector <Circle> &_rslt, int _thresh)
{       
        for ( int i = 0 ; i < nbFrameA ; i++ )
                for ( int j = 0 ; j < nbFrameB ; j++ )
                        for ( int k = 0 ; k < nbFrameR ; k++ )
                                if ( this->getScore(i,j,k) > _thresh )
                                {
                                        _rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
                                                                                           ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
                                                                                           ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
                                                                                         ));
                                }
}

vector<Circle> HoughSpace::threshold(int _thresh)
{
        vector<Circle> rslt;
        
        rslt.clear();
        
        for ( int i = 0 ; i < nbFrameA ; i++ )
                for ( int j = 0 ; j < nbFrameB ; j++ )
                        for ( int k = 0 ; k < nbFrameR ; k++ )
                        {
                                if ( this->getScore(i,j,k) > _thresh )
                                {
                                        rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
                                                                                           ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
                                                                                           ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
                                                                                         ));
                                }
                        }

        return rslt;
}

vector<Circle> HoughSpace::threshold(float _thresh1, float _thresh2)
{
        vector<Circle> rslt;
        int thresh;
        
        rslt.clear();
        
        for ( int i = 0 ; i < nbFrameA ; i++ )
                for ( int j = 0 ; j < nbFrameB ; j++ )
                        for ( int k = 0 ; k < nbFrameR ; k++ )
                        {
                                thresh = (int)((((1.0/(float)scoreTotal)* (_thresh2 - _thresh1)) + _thresh1) / 100.0) * scoreTotal;

                                if ( this->getScore(i,j,k) > thresh )
                                {
                                        rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
                                                                                           ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
                                                                                           ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
                                                                                         ));
                                }
                        }

        return rslt;
}

void HoughSpace::increment(int _a, int _b, int _r)
{
        this->frames[_a][_b][_r].incrementScore();
        this->scoreTotal ++;
}

void HoughSpace::increment ( int _a, int _b, int _r , int _val)
{
        this->frames[_a][_b][_r].incrementScore(_val);
        this->scoreTotal += _val;
}

void HoughSpace::addCircle(Circle _c)
{
	_c.setABR(_c.getA() - getBot().getX(), _c.getB() - getBot().getY(), _c.getR() - getBot().getZ());
	
	int indexA = abs((int)((_c.getA())/getFrameSizeA()));
	int indexB = abs((int)((_c.getB())/getFrameSizeB()));
	int indexR = abs((int)((_c.getR())/getFrameSizeR()));

	if ( indexA >= getNbFrameA() )
			indexA= getNbFrameA()-1;
	
	if ( indexB >= getNbFrameB() )
			indexB = getNbFrameB()-1;
	
	if ( indexR >= getNbFrameR() )
			indexR = getNbFrameR()-1;

	increment( indexA, indexB, indexR );
	
	if( getScore( indexA, indexB, indexR ) > getScoreMax())
	{
			setScoreMax( getScore( indexA, indexB, indexR ));
			setAFrameMax( getHoughFrame( indexA, indexB, indexR ).getBotPoint().getX() + getFrameSizeA()/2);
			setBFrameMax( getHoughFrame( indexA, indexB, indexR ).getBotPoint().getY() + getFrameSizeB()/2);
			setRFrameMax( getHoughFrame( indexA, indexB, indexR ).getBotPoint().getZ() + getFrameSizeR()/2);
	}
	
}

void HoughSpace::saveCircles( vector<Circle> _circles, const QString& _fileName )
{
	ofstream file ( _fileName.toStdString().c_str(), ios::out | ios::trunc);
	
	if( !file )
	{
		cout<<"Erreur lors de l'enregistrement dans \""<<_fileName.toStdString()<<"\""<<endl;
		return;
	}
	
	file<<_circles.size()<<std::endl;
	
	for ( unsigned int i = 0 ; i < _circles.size() ; i++ )
	{
		file<<_circles[i].getA()<<" "<<_circles[i].getB()<<" "<<_circles[i].getR()<<std::endl;
	}
	
	file.close();
}
